/*************************************************************************
    Projet  -  Sport Collectif pour Robots
                             -------------------
    Auteur  -  Basile Wolfrom
*************************************************************************/

//---------------------------------------------------------------- INCLUDE

//-------------------------------------------------------- Include systeme
#include <stdio.h>
#include <stdlib.h>
#include <getopt.h> // Cette librairie est utilisée pour parser les commandes UNIX typiques
#include <unistd.h> // Fournit l'interface de programmation des systèmes d'exploitation POSIX
#include <iostream>
#include <cwchar> // Définit plusieurs fonctions pour travailler avec des chaines C larges
#include <cmath>
#include <map>
#include <log4cplus/logger.h>
#include <log4cplus/configurator.h>

#ifdef __APPLE__	// mac
#include <opencv/openCV.h>
#include <opencv/highgui.h>
#endif

#ifdef __linux__	// gcc (egcs) on linux
#include <opencv.hpp>
#include <highgui/highgui.hpp>
#endif

using namespace std;
using namespace log4cplus;

//------------------------------------------------------ Include personnel
#include "config.h"
#include "util.h"
#include "Vision/VideoSource.h"
#include "IA/Strategie.h"
#include "Controle/ControleRobot.h"

//----------------------------------------------------- Variables globales
Logger loggerMain = Logger::getInstance("main");
Logger loggerVision = Logger::getInstance("Vision");
Logger loggerIA = Logger::getInstance("IA");
Logger loggerControle = Logger::getInstance("Controle");
Logger loggerCom = Logger::getInstance("Communication");

//------------------------------------------------------------- Constantes
static string errorStr = "(option -h ou --help pour plus d'informations)";
static struct option long_options[] = {
		{ HELP,     0, 0, HELP_SHORT },
		{ COMPORT1, 1, 0, 0 },
		{ COMPORT2, 1, 0, 0 },
		{ LOGLVL,   1, 0, LOGLVL_SHORT },				// niveau de log par defaut
		{ LOGLVL_MAIN, 1, 0, 0 },
		{ LOGLVL_VISION, 1, 0, 0 },
		{ LOGLVL_IA,     1, 0, 0 },
		{ LOGLVL_CONTROL,1, 0, 0 },
		{ LOGLVL_COM,1, 0, 0 }
};
static string optString = "hl:";
static map<string, string> cmdLineArgs;

// Chemin des fichiers device des ports COM serie utilises pour communiquer avec les
// robots (/dev/ttyUSB*)
static string comPort1 = PORT_UNKNOWN;
static string comPort2 = PORT_UNKNOWN;


//---------------------------------------------------------- Fonctions CLI

// Affichage du menu d'aide du programme
// Remarque : il est Inutile de proteger cette fonction des acces concurrents
void printHelpMenu() {
	cout << endl;
	cout << "Name : " << "SCOR" << " - Sport Collectif pour Robots" << endl << endl;
	cout << "Description : " << endl << endl;
	cout << "Le programme SCOR est dedie à la gestion de matchs de robots Khepera II. Deux equipes de deux ";
	cout << "robots s’affrontent pour un jeu tenant à la fois du pong, du volley et du football. Cette ";
	cout << "realisation entre dans le cadre d’un projet collectif de cinquième année du département ";
	cout << "informatique de l’INSA de Lyon." << endl << endl;
	cout << "Command line : ";
	cout << "SCOR --comPort1=COMPORT1 --comPort2=COMPORT2 [option]..." << endl << endl;
	cout << "Options : " << endl;
	cout << "\t" << "--help" << "\t\t\t: " << "affiche ce message" << endl;
	cout << "\t" << "--comPort1=COMPORT1" << "\t: " << "COMPORT1 fichier device du port COM robot 1" << endl;
	cout << "\t" << "--comPort1=COMPORT2" << "\t: " << "COMPORT2 fichier device du port COM robot 2" << endl;
	cout << endl;
	cout << "\t" << "Gestion des niveaux de log :" << endl;
	cout << "\t" << "arg=(TRACE | DEBUG | INFO | WARN | ERROR | FATAL)" << endl;
	cout << endl;
	cout << "\t" << "(--LOGLVL | -l)=arg" << "\t: " << "specifie le niveau de log global" << endl;
	cout << "\t" << "--LOGLVL_VISION=arg" << "\t: " << "niveau de log du module \"Vision\"" << endl;
	cout << "\t" << "--LOGLVL_IA=arg" << "\t\t: " << "niveau de log du module \"IA\"" << endl;
	cout << "\t" << "--LOGLVL_CONTROL=arg" << "\t: " << "niveau de log du module \"Controle\"" << endl;
	cout << "\t" << "--LOGLVL_COM=arg" << "\t: " << "niveau de log du module \"Communication\"" << endl;
	cout << "\t" << "--LOGLVL_MAIN=arg" << "\t: " << "niveau de log du module principal" << endl;
	cout << endl;
}

// Analyse syntaxique des parametres sur la ligne de commande
bool processCmdLineArgs(int argc, char **argv) {

	bool cr = true;
	int c, option_index;
	// Desactivation des messages d'erreur de getopt
	opterr = 0;

	for(;;) {
		option_index = 0;
		c = getopt_long(argc, argv, optString.c_str(), long_options, &option_index);
		if (c == -1) {
			break;
		}

		switch (c) {
		case 0:
			// Option longue
			// Sauvegarde des arguments
			cmdLineArgs[long_options[option_index].name] = (optarg ? optarg:NULL);
			break;
		case HELP_SHORT:
			printHelpMenu();
			break;
		case LOGLVL_SHORT:
			// Sauvegarde des arguments
			cmdLineArgs[LOGLVL] = (optarg ? optarg:NULL);
			break;
		case '?':
			if (optopt == '0') {
				// Option longue necessitant un argument
			} else {
				// Option courte
				std::ostringstream oss;
				oss << (char)optopt;
				if (optString.find(optopt) == std::string::npos) {
					cerr << "Option '-" << oss.str() << "' non reconnue " << errorStr << "." << endl;
				}
				else {
					cerr << "L'option '-" << oss.str() << "' necessite un argument " << errorStr << "." << endl;
				}
			}
			cr = false;
			break;
		default:
			std::ostringstream oss;
			oss << c;
			cerr << "?? caractere de code 0" << oss.str() << " ?? " << errorStr << "." << endl;
			cr = false;
			break;
		}
	}
	if (optind < argc) {
		cerr << "Arguments ne constituant pas des options: ";
		while (optind < argc) {
			cerr << "'" << argv[optind++] << "'" << " ";
		}
		cerr << errorStr << "." << endl;
		cr = false;
	}
	return cr;
}


LogLevel getLogLevelCode(string strLogLvl) {
	LogLevel code = -1;
	if(strLogLvl == LOG_NONE) {
		code = OFF_LOG_LEVEL;
	} else if (strLogLvl == LOG_TRACE) {
		code = TRACE_LOG_LEVEL;
	} else if (strLogLvl == LOG_DEBUG) {
		code = DEBUG_LOG_LEVEL;
	} else if (strLogLvl == LOG_INFO) {
		code = INFO_LOG_LEVEL;
	} else if (strLogLvl == LOG_WARN) {
		code = WARN_LOG_LEVEL;
	} else if (strLogLvl == LOG_ERROR) {
		code = ERROR_LOG_LEVEL;
	} else if (strLogLvl == LOG_FATAL) {
		code = FATAL_LOG_LEVEL;
	}
	return code;
}

// Analyse semantique des parametres en ligne de commande
bool validateCmdLineArgs() {

	map<string,string>::iterator it1, it2;

	// Verifier la presence des arguments obligatoires
	if ( (it1=cmdLineArgs.find( COMPORT1 )) == cmdLineArgs.end()
			|| (it2=cmdLineArgs.find( COMPORT2 )) == cmdLineArgs.end() ) {
		cerr << "Il manque des arguments obligatoires (comPort1 ou comPort2) " << errorStr << "." << endl;
	  return false;
	}

	// Verifier la validite des arguments de port COM
	string comPortStr = "/dev/ttyUSB";
	if(it1->second.find(comPortStr) == std::string::npos
			|| it2->second.find(comPortStr) == std::string::npos
			|| it1->second == it2->second) {
		cerr << "Le chemin vers le fichier device du port COM est invalide " << errorStr << "." << endl;
		return false;
	}

	// Verifier la validite des arguments relatifs aux niveaux de log
	for( it1 = cmdLineArgs.begin(); it1 != cmdLineArgs.end(); ++it1 ) {
		if(it1->first.find(LOGLVL) != std::string::npos) {
			// L'option est relative au log
			if( getLogLevelCode(it1->second) == -1 ) {
				// La valeur de l'option est invalide
				cerr << "Le niveau de log specifie n'existe pas " << errorStr << "." << endl;
				return false;
			}
		}
	}

	return true;
}


//------------------------------------------------------------------- MAIN

int main(int argc, char** argv)
{

	//Process command line parameters
	if(!processCmdLineArgs (argc, argv) || !validateCmdLineArgs()) {
		exit(ERROR_CMD_LINE_PARAM);
	}

	//// Debut Configuration des objets dedies au log
	BasicConfigurator config;
	config.configure();
	// Set default log levels
	loggerMain.setLogLevel(LOG_DEFAULT_LVL);
	loggerVision.setLogLevel(LOG_DEFAULT_LVL);
	loggerIA.setLogLevel(LOG_DEFAULT_LVL);
	loggerControle.setLogLevel(LOG_DEFAULT_LVL);
	loggerCom.setLogLevel(LOG_DEFAULT_LVL);
	//// Fin Configuration des objets dedies au log

	// Mise à jour des variables de configuration
	comPort1 = cmdLineArgs.find( COMPORT1 )->second;
	comPort2 = cmdLineArgs.find( COMPORT2 )->second;

	// Initialiser les niveaux de log
	map<string,string>::iterator it;
	LogLevel logLvl;
	if ( (it=cmdLineArgs.find( LOGLVL )) != cmdLineArgs.end() ) {
		// Surcharge des niveaux de log avec le niveau par defaut
		logLvl = getLogLevelCode(it->second);
		loggerMain.setLogLevel(logLvl);
		loggerVision.setLogLevel(logLvl);
		loggerIA.setLogLevel(logLvl);
		loggerControle.setLogLevel(logLvl);
		loggerCom.setLogLevel(logLvl);
		//cout << "loggers set to log level : " << logLvl << endl;
	}
	string curLogOption;
	for( it = cmdLineArgs.begin(); it != cmdLineArgs.end(); ++it ) {
		curLogOption = it->first;
		if(curLogOption.find(LOGLVL) != std::string::npos && curLogOption != LOGLVL) {
			logLvl = getLogLevelCode(it->second);
			if(curLogOption == LOGLVL_VISION) {
				loggerVision.setLogLevel(logLvl);
				//cout << "loggerVision.setLogLevel(" << logLvl << ")" << endl;
			} else if (curLogOption == LOGLVL_IA) {
				loggerIA.setLogLevel(logLvl);
				//cout << "loggerIA.setLogLevel(" << logLvl << ")" << endl;
			} else if (curLogOption == LOGLVL_CONTROL) {
				loggerControle.setLogLevel(logLvl);
				//cout << "loggerControle.setLogLevel(" << logLvl << ")" << endl;
			} else if (curLogOption == LOGLVL_COM) {
				loggerCom.setLogLevel(logLvl);
				//cout << "loggerCom.setLogLevel(" << logLvl << ")" << endl;
			} else if (curLogOption == LOGLVL_MAIN) {
				loggerMain.setLogLevel(logLvl);
				//cout << "loggerMain.setLogLevel(" << logLvl << ")" << endl;
			}
		}
	}

	LOG4CPLUS_INFO(loggerMain, "SCOR program has started !");

	//Creation des objets de communication
	MsgQueue<DataVision> videoToControl;
	MsgQueue<DataVision> videoToControl2;
	MsgQueue<DataVision> videoToIA;
	MsgQueue<DataIA> IAToControl_RG;
	MsgQueue<DataIA> IAToControl_RD;

	list<MsgQueue<DataVision>*> visionQueues;
	visionQueues.push_back(&videoToControl);
	visionQueues.push_back(&videoToControl2);
	visionQueues.push_back(&videoToIA);

	VideoSource vision(LARGEUR_TERRAIN_IMAGE_X, LONGUEUR_TERRAIN_IMAGE_Y, visionQueues);
	Strategie strategie(&IAToControl_RG, &IAToControl_RD, &videoToIA);
	//ATTENTION : relation comPortX - robots G/D
	ControleRobot controleRobot1(comPort1, ROBOT_GAUCHE, &videoToControl, &IAToControl_RG);
	ControleRobot controleRobot2(comPort2, ROBOT_DROITE, &videoToControl2, &IAToControl_RD);

	//Phase de configuration des coins du terrain
	if(!vision.InitField())
	{
		LOG4CPLUS_INFO(loggerMain, "Echec de l'initialisation du terrain ! Probablement dû à un problème de détection de la caméra.");
		goto cleanup;
	}

	// Lancement
	LOG4CPLUS_INFO(loggerMain, "Lancement de la stratégie.");
	strategie.LancerStrategie();

	if(!controleRobot1.LancerControle())
	{
		LOG4CPLUS_ERROR(loggerMain, "Echec du lancement du controle robot 1.");
		goto cleanup;
	}
	if(!controleRobot2.LancerControle())
	{
		LOG4CPLUS_ERROR(loggerMain, "Echec du lancement du controle robot 2.");
		goto cleanup;
	}
	if(!vision.LancerAnalyse())
	{
		LOG4CPLUS_ERROR(loggerMain, "Echec du lancement du thread d'analyse d'image.");
		goto cleanup;
	}

	usleep(3*1000*1000);
	LOG4CPLUS_INFO(loggerMain, "PRESS ANY KEY TO EXIT...");
	getchar();

	controleRobot1.ArreterControle();
	controleRobot2.ArreterControle();

	if(strategie.ArreterStrategie() != 0)
	{
		LOG4CPLUS_ERROR(loggerMain, "Erreur d'arret thread strategie");
	}
	if(vision.ArreterAnalyse() != 0)
	{
		LOG4CPLUS_ERROR(loggerMain, "Erreur d'arret thread video");
	}

cleanup:

	LOG4CPLUS_INFO(loggerMain, "SCOR program has terminated !");
	return 0;
}
